#include "twister.h"
#include "emmintrin.h"

namespace Tmpl {

#define MT_N 624
#define MT_M 397
#define MT_MATRIX_A   0x9908b0dfUL
#define MT_UPPER_MASK 0x80000000UL
#define MT_LOWER_MASK 0x7fffffffUL

#define MALLOC64(size, type)		((type*)_aligned_malloc((size) * sizeof(type), 64))
#define FREE64(ptr)					_aligned_free(ptr)

static __m128i  MTUpperMask;
static __m128i  MTLowerMask;
static __m128i  MTMatrixA;
static __m128i  One;
static __m128   Scale;	    // default gives random numbers in [0..1)

#define  K_2M31  (2.0/4294967296.0)	// default scale => [0..1)

void Twister::RandomInit(unsigned int s, float scale) 
{
    int mti;
    mtState[0]= s & 0xffffffffUL;
    for (mti=1; mti<MT_N; ++mti) 
	{
        mtState[mti] = (1812433253UL * (mtState[mti-1] ^ (mtState[mti-1] >> 30)) + mti); 
        /* See Knuth TAOCP Vol2. 3rd Ed. P.106 for multiplier. */
        /* In the previous versions, MSBs of the seed affect   */
        /* only MSBs of the array mt[].                        */
        /* 2002/01/09 modified by Makoto Matsumoto             */
        mtState[mti] &= 0xffffffffUL;
        /* for >32 bit machines */
    }
    for (mti=MT_N; mti<MT_N+8; mti++)	    // initialize fetch-ahead words to
	mtState[mti] = 0;		    // prevent memory checker complaints

    mtFloatCounter = MT_N;
    MTUpperMask = _mm_set1_epi32(MT_UPPER_MASK);
    MTLowerMask = _mm_set1_epi32(MT_LOWER_MASK);
    MTMatrixA   = _mm_set1_epi32(MT_MATRIX_A);
    One         = _mm_set1_epi32(1);
    Scale       = _mm_set_ps1(scale);
}

void Twister::NewRandomState(unsigned int *state, float *output)
{
    register int i, m;
    register __m128i  *sp, y01, y12, mag01, mag12;
    register __m128i   ws0, ws1, ws2, ws01, ws12;
    register __m128i   wm0, wm1, wm2, wm01, wm12;
    register __m128i   t1, t2, t3, t4, t5, t6;
    register __m128   *tjp, yf01, yf12;

    sp  = (__m128i *)state;
    tjp = (__m128  *)output;

    m = MT_M/4;

    ws0 = sp[0];
    wm0 = sp[m];

    // 0-1 part, first half (startup only)

    ws1 = sp[1];
    wm1 = sp[m+1];

    ws01 = _mm_or_si128 (((__m128i)_mm_srli_si128(ws0, 4)),
                         ((__m128i)_mm_slli_si128(ws1, 12)));

    t1  = _mm_and_si128 (ws0, MTUpperMask);	    // shift 1 bit
    t2  = _mm_and_si128 (ws01, MTLowerMask);
    y01 = _mm_or_si128 (t1, t2);

    mag01 = _mm_and_si128 (y01, One);	    // conditional xor
    mag01 = _mm_cmpeq_epi32 (mag01, One);
    mag01 = _mm_and_si128 (mag01, MTMatrixA);

    for (i = 0; i < MT_N/4; i+=2, m+=2) {	    // MT_N (==624) is a multiple of 4

    // 1-2 part, first half

        ws2 = sp[i+2];				    // 1-2 part, first half
        wm2 = sp[m+2];
						    // extract [1234] from [0123] and [4567]
        ws12 = _mm_or_si128 (((__m128i)_mm_srli_si128 (ws1, 4)),
                             ((__m128i)_mm_slli_si128 (ws2, 12)));

        t4   = _mm_and_si128 (ws1,  MTUpperMask);	// shift 1 bit
        t5   = _mm_and_si128 (ws12, MTLowerMask);
        y12  = _mm_or_si128 (t4, t5);

        mag12 = _mm_and_si128 (y12, One);    // conditional xor
        mag12 = _mm_cmpeq_epi32 (mag12, One);
        mag12 = _mm_and_si128 (mag12, MTMatrixA);

    // 0-1 part, second half
						    // extract [1234] from [0123] and [4567]
        wm01 = _mm_or_si128 (((__m128i)_mm_srli_si128 (wm0, 4)),
                             ((__m128i)_mm_slli_si128 (wm1, 12)));
	t1 = _mm_srli_epi32 (y01, 1);
        t2 = _mm_xor_si128 (t1, mag01);
        t3 = _mm_xor_si128 (wm01, t2);

        sp[i] = t3;				    // new 0-1 state

        t3     = _mm_srli_epi32 (t3, 1);
        yf01   = _mm_cvtepi32_ps (t3);
        tjp[i] = _mm_mul_ps (yf01, Scale);

    // 1-2 part, second half
						    // extract [1234] from [0123] and [4567]
        wm12 = _mm_or_si128 (((__m128i)_mm_srli_si128 (wm1, 4)),
							 ((__m128i)_mm_slli_si128 (wm2, 12)));
        t4 = _mm_srli_epi32 (y12, 1);
        t5 = _mm_xor_si128 (t4, mag12);
        t6 = _mm_xor_si128 (wm12, t5);

        sp[i+1] = t6;				    // new 1-2 state

        t6       = _mm_srli_epi32 (t6, 1);	    // convert upper 31 bits to float
        yf12     = _mm_cvtepi32_ps (t6);
        tjp[i+1] = _mm_mul_ps (yf12, Scale);	    // scale to [0..1) and save

        if (m == MT_N/4-3)  {	    // did we just fetch the last m?
	   m = -3;		    // the next fetched sp[m+3] needs to come from sp[0]
	   state[MT_N] = state[0];  // SIMD wrap padding to compute state[MT_N-1]
        }

        ws0 = ws2;				    // scroll forward by 8
        wm0 = wm2;

    // 0-1 part, first half

        ws1 = sp[i+3];
        wm1 = sp[m+3];
						    // shift 1 word left
        ws01 = _mm_or_si128 (((__m128i)_mm_srli_si128 (ws0, 4)),
                             ((__m128i)_mm_slli_si128 (ws1, 12)));
        t1 = _mm_and_si128 (ws0, MTUpperMask);	    // shift 1 bit
        t2 = _mm_and_si128 (ws01, MTLowerMask);
        y01 = _mm_or_si128 (t1, t2);

        mag01 = _mm_and_si128 (y01, One);	    // conditional xor
        mag01 = _mm_cmpeq_epi32 (mag01, One);
        mag01 = _mm_and_si128 (mag01, MTMatrixA);
    }
}

void Twister::Init()
{
	mtScale = 1.0f;
    RandomInit(5489UL, K_2M31); // Init the tables
}
void Twister::Init(float scale)
{
	mtScale = scale;
    RandomInit(5489UL, float(scale*K_2M31)); // Init the tables
}
void Twister::Init(unsigned int seed, float scale)
{
	mtScale = scale;
    RandomInit(seed, float(scale*K_2M31)); // Init the tables
}

//////////// MTStore Constructor/Destructor

Twister::Twister(void) 
{
	mtStateBase = MALLOC64(MT_N+10, unsigned int);
	mtState = mtStateBase;
	mtFloatBase = MALLOC64(MT_N+2, float);
	mtFloat = mtFloatBase;

    mtFloatCounter = MT_N;
}

Twister::~Twister(void) 
{
	if (mtStateBase) FREE64(mtStateBase);
	if (mtFloatBase) FREE64(mtFloatBase);
}
// Get random function
float Twister::GetRandom()
{
	float* rn = (float*)mtFloat;
	unsigned short c = mtFloatCounter;
	if (++c >= MT_N)  
	{
		NewRandomState(mtState, rn);
		mtFloatCounter = 0;
		return rn[0];
	}  
	else  
	{
		mtFloatCounter = c;
		return rn[c];
	}
}
float Twister::GetRandomNegative()
{
	return GetRandom()-GetRandom();
}
float Twister::GetRandomRange(float min, float max)
{
	float value = GetRandom() / mtScale;
	return min + (max-min)*value;
}


};
